#ifndef INC_5_FINAL_SOLUTION_H
#define INC_5_FINAL_SOLUTION_H
#include "cmath"
#include <array>
#include <chrono>
#include <cstring>
#include <iostream>
#include <queue>
#include <set>
#include <stack>
#include <tuple>
#include <vector>
#include <utility>
#include <opencv2/opencv.hpp>

using namespace std;
using namespace cv;

typedef pair<int, int> Pair;
typedef vector<vector<int>> Vec2int;
typedef tuple<bool, int, int, int> PathVar;

class Solution {
private:
    int ROW;
    int COL;

    Pair StartPoint;
    Pair EndPoint;
    stack<Pair> DynamicPath;
    Vec2int map_vector;
    Vec2int radar_vector;
    Vec2int check_vector;
    Vec2int show_vector;

public:
    Solution() :
        ROW{ 100 },
        COL{ 100 },
        map_vector(ROW, vector<int>(COL)),
        radar_vector(ROW, vector<int>(COL)),
        check_vector(ROW, vector<int>(COL)),
        show_vector(ROW, vector<int>(COL)),
        StartPoint({ 10, 10 }),
        EndPoint({ 90, 90 })
    {}
    struct cell {
        Pair nodeParent;
        double cost_f, cost_g, cost_h;
        cell() :
            nodeParent(-1, -1), cost_f(-1), cost_g(-1), cost_h(-1) {}
    };

    void setStartPoint(int x, int y);
    static Pair cor2ArrayPos(Pair corPos);
    bool isValid(const vector<vector <int>>& grid, const Pair& point) const;
    bool isUnBlocked(const vector<vector <int>>& grid, const Pair& point) const;
    bool isDestination(const Pair& position);

    static double calculateHValue(const Pair& src, const Pair& dest);

    Mat vector2ObstacleMap(Vec2int& grid) const;
    Vec2int getVectorRawGrid(const string& mapName);
    Vec2int getVectorBoundaryGrid(Vec2int& grid) const;
    static vector<vector<int>> mat2Vector(Mat& src, int m, int n);
    static Mat vector2Mat(vector<vector<int>>& vec);
    void fetchLidarData(Pair& pos, Vec2int& Vec);
    static vector<pair<double, double>> mergeInterval(const pair<double, double>& newRange,

    static bool checkInterval(const pair<double, double>& newRange,
        vector<pair<double, double>>& intervals);

    static pair<double, double> getThetaRange(Pair point_0, Pair point_d);
    void clearRadarMap();
    void launchRadar();
    void dynamicTracePath(const vector<vector <cell>>& cellDetails, const Pair& pos);

    PathVar ifPathObstacle(vector<Pair>& path);
    void showPath();
};


#endif //INC_5_FINAL_SOLUTION_H